/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; version 2.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
/**
 * @file: epuck_informed_coverage.h
 * @brief Improved controller to allocate robot to spatially distributed tasks (stimuli-based)
 *
 * The current controller makes use only of the information sensed using the camera and the proximity sensors to
 * allocate e-pucks into spatially distributed booths.
 *
 * @date: March 18, 2013
 *
 * @author Jacopo De Stefani <jacopo.de.stefani@ulb.ac.be>
 * @author Manuele Brambilla <mbrambilla@iridia.ulb.ac.be>
 *
 */

#ifndef EPUCK_INFORMED_COVERAGE_CONTROLLER_H
#define EPUCK_INFORMED_COVERAGE_CONTROLLER_H

//#include "config.h"
#include <argos2/common/control_interface/ci_controller.h>
#include <argos2/common/control_interface/e-puck/ci_epuck_proximity_sensor.h>
#include <argos2/common/control_interface/e-puck/ci_epuck_encoder_sensor.h>
#include <argos2/common/control_interface/e-puck/ci_epuck_omnidirectional_camera_sensor.h>
#include <argos2/common/control_interface/e-puck/ci_epuck_range_and_bearing_sensor.h>
#include <argos2/common/control_interface/e-puck/ci_epuck_leds_actuator.h>
#include <argos2/common/control_interface/e-puck/ci_epuck_wheels_actuator.h>
#include <argos2/common/control_interface/e-puck/ci_epuck_range_and_bearing_actuator.h>
#include <argos2/common/utility/argos_random.h>
#include <argos2/common/utility/math/vector2.h>
#include <argos2/common/utility/math/angles.h>
#include <argos2/common/utility/logging/argos_log.h>
#include <argos2/simulator/physics_engines/physics_engine.h>

#include <algorithm>

using namespace argos;

class CEPuckInformedCoverageController : public CCI_Controller {
    // Associations

public:

	CEPuckInformedCoverageController();

	virtual ~CEPuckInformedCoverageController();

	virtual void Init(TConfigurationNode& t_tree);

	virtual void ControlStep();

	virtual void Destroy();

	/* TAM informations */
	struct SClusterExpectedPosition {
			UInt16 m_unClusterId;
	        CVector2 m_cPositionWRTRobot;
	        CARGoSRandom::CRNG* m_pcEncoderRNG;

	        SClusterExpectedPosition() :
	        	m_unClusterId(-1),
	        	m_cPositionWRTRobot(){
	        	m_pcEncoderRNG = CARGoSRandom::CreateRNG("argos");
	        }

	        void UpdatePosition(Real,Real,Real);
	        void UpdatePositionWithoutEncoders(Real,Real,Real);
	};

	/****************************************/
	/*			Controller states			*/
	/****************************************/
    enum EnFSM {
    			LINEAR_WALK,
    			RANDOM_TURN,
    			DIRECTING_TO_TAM,
    			ASSESSING_CLUSTER,
    			INTO_TAM,
    			WORK,
    			EXIT_TAM,
     };

    virtual EnFSM GetState();


    virtual void ChooseNextPrey();

    bool m_obstacleSensed;
    bool m_hasPrey1;
    bool m_hasPrey2;
    UInt32 m_targetPrey;
    bool m_prey1Trigger;
    bool m_prey2Trigger;
    CVector2 m_cCurrentPosition;

    std::string m_sBehaviorKind;

    void SetClusterInformation(UInt16,UInt16,UInt16,std::string);

    /****************************************/
    /*				Motion control			*/
    /****************************************/
    void SetDesiredDirection(CVector2& c_direction);
    CVector2 GetDesiredDirection() const;

    /****************************************/
    /*				Random walk				*/
    /****************************************/
    void SetChangeDirectionProbability(Real f_change_direction_probability);
    Real GetChangeDirectionProbaility() const;

    /****************************************/
    /*				Blob tracking			*/
    /****************************************/
    bool IsTamAvailable() const;
	const SClusterExpectedPosition& GetClosestTamExpectedPosition() const;

protected:

    static bool myBlobsSort(CCI_CameraSensor::SBlob* blob1, CCI_CameraSensor::SBlob* blob2);

private:

	EnFSM m_enFMS;
	size_t m_unTimesteps;

	/****************************************/
	/*				Actuators				*/
	/****************************************/
    CCI_EPuckWheelsActuator* m_pcWheelsActuator;
    CCI_EPuckLedsActuator* m_pcLedActuator;
    CCI_EPuckRangeAndBearingActuator* m_pcRangeAndBearingActuator;

    /****************************************/
    /*				Sensors					*/
    /****************************************/
    CCI_EPuckOmnidirectionalCameraSensor* m_pcCameraSensor;
    CCI_EPuckProximitySensor* m_pcProximitySensor;
    CCI_EPuckEncoderSensor* m_pcEncoderSensor;
    CCI_EPuckRangeAndBearingSensor* m_pcRangeAndBearing;

    /****************************************/
    /*				XML Parameters			*/
    /****************************************/
    CARGoSRandom::CRNG* m_pcRNG;

    Real m_wheelSpeed;
    UInt8 m_randomWalkTurns;
    SInt8 m_nRotationSteps;

    Real m_fProximityWeight;
    Real m_fProximityThreshold;
    Real m_fCameraThreshold;
    Real m_fEncoderError;
    CVector2 m_closestObstacleReversed;

    UInt32 m_nRandomTurnSteps;
    UInt32 m_unGoToStopSteps;
    UInt32 m_unStopSteps;
    SInt32 m_nExplorationSteps;

    /****************************************/
    /*				Random walk				*/
    /****************************************/

    /* Change direction probability */
    Real m_fChangeDirectionProbability;
    /* Change direction max value */
    Real m_fMaxRandomDirectionValue;
    /* Desired direction vector */
    CVector2 m_cDesiredDirection;

     /**
     * Calculate random direction change by applying a
     * uniform(0,1) sample to the vector of available direction
     * change ranges.
     */
    void CalculateRandomDirectionChange();

    /****************************************/
    /*				Motion control			*/
    /****************************************/

    enum ERotationDirection{
    	CLOCKWISE,
    	COUNTERCLOCKWISE,
    };

    ERotationDirection m_eRotationDirection;

    Real m_fLeftSpeed;
    Real m_fRightSpeed;

    /* Linear speed */
    Real m_fLinearSpeed;
    /* Angular speed */
    Real m_fAngularSpeed;

    Real m_fPac_min;
    UInt16 m_fPac_exp;
    UInt16 m_unStopStepsDefault;

    bool m_bIsSensingLateralWall;
    bool m_bChangeDirection;
    UInt16 m_unTurningSteps;


    CRange<UInt32> cRandomRange;
    CRange<Real> cRandomPac;
    CRange<CRadians> m_cRandomTurnThreshold;

    Real m_fExplorationDuration;
    Real m_fExplorationBlindPeriod;
    SInt32 m_nBlindSteps;
    SInt32 m_nBlockedSteps;

    /* Saturate linear speed to maximum value */
    Real SaturateLinearSpeed();
    void ActuateWheels();
    void ComputeSpeedFromAngle(CRadians c_target_angle);

    void ParseProximity();
    CVector2 RescaleProximity();


    /****************************************/
    /*				Blob tracking			*/
    /****************************************/

    const CCI_CameraSensor::SCameraReadings* m_sCameraReadings;

    void ParseCamera();

    bool m_isBoothAvailable;
    bool m_isClosestBoothBusy;
    bool m_isClosestBoothUnavailable;
    bool m_isClosestBoothDisabled;
    CCI_CameraSensor::SBlob* m_pcClosestAvailableBoothBlob;
    CCI_CameraSensor::SBlob* m_pcClosestOtherBoothBlob;

    CColor m_preyColor;

    /****************************************/
    /*				Odometry				*/
    /****************************************/
    // Area structure, for the nest and source
    void ParseEncoder();

    SClusterExpectedPosition m_sLastVisitedClusterPosition;
    Real m_fEncoderDistanceLeftWheel;
    Real m_fEncoderDistanceRightWheel;
    bool m_bIsTrackingPosition;


    /****************************************/
    /*				Cluster information		*/
    /****************************************/
    struct SClusterInformation{
    					  UInt16 clusterId;
    					  std::string tamId;
          			      UInt16 clusterRequest;
          			      UInt16 clusterOccupation;
          			      bool isValid;

          			      SClusterInformation():
          			    	  clusterId(0),
          			    	  tamId(),
          			    	  clusterRequest(0),
          			      	  clusterOccupation(0),
          			      	  isValid(false){}

          			      void Init(TConfigurationNode& t_node);

    };
    SClusterInformation m_sClusterInformation;
    bool m_bIsOccpuationChanged;
    bool m_bDirectingSignaled;

    /****************************************/
    /*				Decision phase			*/
    /****************************************/
    void TakeDecision();

    /****************************************/
    /*				Range and Bearing		*/
    /****************************************/
    void ParseRAB();
    void SendRAB(UInt8,UInt8);

    struct SNeighborRobotsInformation{
    	UInt16 m_unRobotsWalking;
    	UInt16 m_unRobotsAssessing;
    	UInt16 m_unRobotsDirecting;
    	UInt16 m_unRobotsWorking;
    	bool m_bIsAnotherRobotDirecting;


    	SNeighborRobotsInformation():
    		m_unRobotsWalking(0),
    		m_unRobotsAssessing(0),
    		m_unRobotsDirecting(0),
    	    m_unRobotsWorking(0),
    	    m_bIsAnotherRobotDirecting(false){}

    	void Reset(){
    		m_unRobotsWalking = 0;
    		m_unRobotsAssessing = 0;
    		m_unRobotsDirecting = 0;
    		m_unRobotsWorking = 0;
    		m_bIsAnotherRobotDirecting = false;
    	}
    };

   SNeighborRobotsInformation m_sNeighborRobots;
};

#endif
